﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.IO.Ports;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using BaseDll;
using Communicate;
using SerialDict;
using MotionIoLib;
using System.Threading;
using System.ComponentModel;
using System.Net;
using System.Net.Sockets;
using CommonTools;

namespace MotionIoLib
{
    public class SixRobot_Estun_Old : BaseSixRobot
    {
        string str_IP地址 = "192.168.6.40";//机器人ip  192.168.6.40
        int n_Port = 5000;
        string str_ClientIP = "192.168.6.69";//机器人ip  192.168.6.69
        bool b_ReceiveMsg = false;
        string str_ServoInput = "";
        string strMeg = "";
        public SixRobot_Estun_Old(string strName, params object[] RobotParam) : base(strName, RobotParam)
        {
            this.strName = strName;
            str_IP地址 = RobotParam[0].ToString();
            n_Port = int.Parse(RobotParam[1].ToString());
            str_ClientIP = RobotParam[2].ToString();
            str_ServoInput = RobotParam[3].ToString();
        }

        private Socket socketCommunication;
        private Dictionary<string, Socket> dict = new Dictionary<string, Socket>();
        private Dictionary<string, Thread> dictThread = new Dictionary<string, Thread>();
        bool b_IsConnect = false;
        public override bool Open()
        {
            try
            {
                Socket socketWatch = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
                IPAddress ip = IPAddress.Parse(str_IP地址);
                IPEndPoint endPoint = new IPEndPoint(ip, n_Port);
                socketWatch.Bind(endPoint);
                socketWatch.Listen(10);
                Thread thread = new Thread(Listen);
                thread.IsBackground = true;
                thread.Start(socketWatch);
                return true;
            }
            catch (Exception)
            {
                return false;
            }

        }
        void Listen(object obj)
        {
            Socket socketWatch = obj as Socket;
            while (true)
            {
                try
                {
                    //等待客户端的连接，同时会创建一个与其通信的socket
                    socketCommunication = socketWatch.Accept();
                    if (!dict.Keys.Contains(socketCommunication.RemoteEndPoint.ToString().Split(':')[0]))
                    {
                        dict.Add(socketCommunication.RemoteEndPoint.ToString().Split(':')[0], socketCommunication);
                        if (dict.ContainsKey(str_ClientIP))
                            b_IsConnect = true;
                    }
                    else
                    {
                        dict[socketCommunication.RemoteEndPoint.ToString().Split(':')[0]] = socketCommunication;
                    }
                    Thread thread = new Thread(RecMsg);
                    thread.IsBackground = true;
                    thread.Start(socketCommunication);
                    dictThread.Add(socketCommunication.RemoteEndPoint.ToString().Split(':')[0], thread); // 将新建的线程 添加 到线程的集合中去。
                }


                catch (Exception ex)
                {
                    MessageBox.Show(ex.Message);
                }
            }
        }

        void RecMsg(object sokConnectionparn)
        {
            Socket sokClient = sokConnectionparn as Socket;
            while (true)
            {
                // 定义一个2M的缓存区；
                // byte[] arrMsgRec = new byte[1024 * 1024 * 2];
                byte[] arrMsgRec = new byte[1024];
                // 将接受到的数据存入到输入 arrMsgRec中；
                int length = -1;
                try
                {
                    length = sokClient.Receive(arrMsgRec); // 接收数据，并返回数据的长度；
                    if (length == 0)
                    {
                        // 从 通信套接字 集合中删除被中断连接的通信套接字；
                        dict.Remove(sokClient.RemoteEndPoint.ToString().Split(':')[0]);
                        // 从通信线程集合中删除被中断连接的通信线程对象；
                        dictThread.Remove(sokClient.RemoteEndPoint.ToString().Split(':')[0]);
                        sokClient.Shutdown(SocketShutdown.Both);
                        sokClient.Close();
                        return;
                    }
                }
                catch (Exception)
                {
                    // 从通信套接字 集合中删除被中断连接的通信套接字；
                    dict.Remove(sokClient.RemoteEndPoint.ToString().Split(':')[0]);
                    // 从通信线程集合中删除被中断连接的通信线程对象；
                    dictThread.Remove(sokClient.RemoteEndPoint.ToString().Split(':')[0]);
                    break;
                }
                //获取接收的消息
                string ReciveMsg = Encoding.UTF8.GetString(arrMsgRec, 0, length).Split('\r', '\n')[0];// 将接受到的字节数据转化成字符串；
                strMeg = ReciveMsg;
                b_ReceiveMsg = true;

            }
        }

        public void Send(string msg)
        {
            if (dict != null && dict.Count > 0)
            {
                Socket socket = dict[str_ClientIP];
                if (socket != null)
                {
                    b_ReceiveMsg = false;
                    byte[] buffer = Encoding.UTF8.GetBytes(msg);
                    socket.Send(buffer);
                }
            }
        }
        object obj_Lock = new object();
        private int Function_SendCmd(string strCmd, ref string strResult, int n_TimeOut = 2000)
        {
            lock (obj_Lock)
            {
                this.Send(strCmd);
                Stopwatch stopwatch = new Stopwatch();
                stopwatch.Restart();
                while (true)
                {
                    Thread.Sleep(10);
                    if (b_ReceiveMsg)
                    {
                        if (!strMeg.Contains('['))
                        {
                            b_ReceiveMsg = false;
                            continue;
                        }
                        strResult = strMeg;
                        return 0;
                    }
                    else if (stopwatch.ElapsedMilliseconds > n_TimeOut)
                    {
                        return -4;
                    }
                }


            }
        }
        //设置负载
        public override bool SetPayload(int Scope, string payloadName)
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd("[SetPayload_IFace(" + Scope.ToString() + ",\"" + payloadName + "\"); id=35]", ref strMeg);
                if (n_Result == 0 && strMeg.Contains("Ok"))
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return false;
            }

        }

        //读取输入信号
        public override bool ReadDigitalInPort(int n_Index)
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd("[IOGetDin(" + n_Index.ToString() + "); id = 1]", ref strMeg);
                if (n_Result == 0 && strMeg.Contains("[id = 1; Ok; 1]"))
                {
                    return true;
                }
                else if (n_Result == 0 && strMeg.Contains("[id = 1; Ok; 0]"))
                {
                    return false;
                }
                else
                {
                    return false;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return false;
            }

        }

        //读取输出信号
        public override bool ReadDigitalOutPort(int n_Index)
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd("[IOGetAout(" + n_Index.ToString() + "); id = 7]", ref strMeg);
                if (n_Result == 0 && strMeg.Contains("[id = 7; Ok; 1]"))
                {
                    return true;
                }
                else if (n_Result == 0 && strMeg.Contains("[id = 7; Ok; 0]"))
                {
                    return false;
                }
                else
                {
                    return false;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return false;
            }

        }

        //设置输出信号
        public override bool SetDigitalOutputPort(int n_Index, bool b_On)
        {
            try
            {
                string strOn = b_On ? "1" : "0";
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd("[SetIOValue(" + n_Index.ToString() + ",1" + "," + strOn + "); id = 1]", ref strMeg);
                if (n_Result == 0 && strMeg.Contains("Ok"))
                {
                    return true;
                }
                else
                {
                    return false;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return false;
            }
        }
        public override SixRobotJoint GetCurrentPos_Joint()
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"[GetCurJPos();id=1]", ref strMeg);
                SixRobotJoint PointWorldObject = new SixRobotJoint(0, 0, 0, 0, 0, 0);
                if (strMeg == null)
                    return PointWorldObject;
                if (strMeg.Contains("Ok"))
                {
                    try
                    {
                        string CurrentPos_Joint = strMeg;
                        int StartPosition = CurrentPos_Joint.IndexOf("Ok") + 4;
                        int EndPosition = CurrentPos_Joint.IndexOf("]");
                        string SubCurrentPos_Joint = CurrentPos_Joint.Substring(StartPosition, EndPosition - StartPosition);
                        string[] SubCurrentPos_JointArray = SubCurrentPos_Joint.Split(new char[] { ' ' });
                        PointWorldObject.A1 = double.Parse(SubCurrentPos_JointArray[0]);
                        PointWorldObject.A2 = double.Parse(SubCurrentPos_JointArray[1]);
                        PointWorldObject.A3 = double.Parse(SubCurrentPos_JointArray[2]);
                        PointWorldObject.A4 = double.Parse(SubCurrentPos_JointArray[3]);
                        PointWorldObject.A5 = double.Parse(SubCurrentPos_JointArray[4]);
                        PointWorldObject.A6 = double.Parse(SubCurrentPos_JointArray[5]);
                    }
                    catch (Exception)
                    {
                        PointWorldObject.A1 = 0;
                        PointWorldObject.A2 = 0;
                        PointWorldObject.A3 = 0;
                        PointWorldObject.A4 = 0;
                        PointWorldObject.A5 = 0;
                        PointWorldObject.A6 = 0;
                    }
                }
                else
                {
                    PointWorldObject.A1 = 0;
                    PointWorldObject.A2 = 0;
                    PointWorldObject.A3 = 0;
                    PointWorldObject.A4 = 0;
                    PointWorldObject.A5 = 0;
                    PointWorldObject.A6 = 0;
                }

                return PointWorldObject;
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}");
                return new SixRobotJoint(0, 0, 0, 0, 0, 0); ;
            }
        }
        //获取当前位置---世界坐标系
        public override SixRobotPoint GetCurrentPos_World()
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"[GetCurWPos();id=2]", ref strMeg);
                SixRobotPoint PointWorldObject = new SixRobotPoint(0, 0, 0, 0, 0, 0);
                if (strMeg == null)
                    return PointWorldObject;
                if (strMeg.Contains("Ok"))
                {
                    try
                    {
                        string CurrentPos_Joint = strMeg;
                        int StartPosition = CurrentPos_Joint.IndexOf("Ok") + 4;
                        int EndPosition = CurrentPos_Joint.IndexOf("]");
                        string SubCurrentPos_Joint = CurrentPos_Joint.Substring(StartPosition, EndPosition - StartPosition);
                        string[] SubCurrentPos_JointArray = SubCurrentPos_Joint.Split(new char[] { ' ' });
                        PointWorldObject.X = double.Parse(SubCurrentPos_JointArray[0]);
                        PointWorldObject.Y = double.Parse(SubCurrentPos_JointArray[1]);
                        PointWorldObject.Z = double.Parse(SubCurrentPos_JointArray[2]);
                        PointWorldObject.W = double.Parse(SubCurrentPos_JointArray[3]);
                        PointWorldObject.P = double.Parse(SubCurrentPos_JointArray[4]);
                        PointWorldObject.R = double.Parse(SubCurrentPos_JointArray[5]);
                    }
                    catch (Exception)
                    {
                        PointWorldObject.X = 0;
                        PointWorldObject.Y = 0;
                        PointWorldObject.Z = 0;
                        PointWorldObject.W = 0;
                        PointWorldObject.P = 0;
                        PointWorldObject.R = 0;
                    }
                }
                else
                {
                    PointWorldObject.X = 0;
                    PointWorldObject.Y = 0;
                    PointWorldObject.Z = 0;
                    PointWorldObject.W = 0;
                    PointWorldObject.P = 0;
                    PointWorldObject.R = 0;
                }

                return PointWorldObject;
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}");
                return new SixRobotPoint(0, 0, 0, 0, 0, 0); ;
            }


        }        //直线运动
        public override bool MoveLine(SixRobotPoint Point, bool bmamual, int Speed = 100, int BlendValue = 500)
        {
            StationCheck(bmamual);
            string Commond = "[MovePointV3(2," + "\"" + Point.X.ToString() + "_" + Point.Y.ToString() + "_" + Point.Z.ToString() + "_" + Point.W.ToString() + "_" + Point.P.ToString() + "_" + Point.R.ToString() + "\"," + "\"" + "\"," + "\"" + "0" + "_" + Speed.ToString() + "_" + BlendValue.ToString() + "\"" + "); id = 3]";
            int n_Result = 0;
            string strMeg = "";
            n_Result = Function_SendCmd(Commond, ref strMeg, 60000);
            if (n_Result == 0 && !strMeg.Contains("FAIL"))
            {
                bool b_Result = true;
                Stopwatch stopwatch = new Stopwatch();
                stopwatch.Start();
                while (true)
                {
                    SixRobotPoint point = GetCurrentPos_World();
                    b_Result = true;
                    b_Result &= Math.Abs(point.X - Point.X) < 0.5;
                    b_Result &= Math.Abs(point.Y - Point.Y) < 0.5;
                    b_Result &= Math.Abs(point.Z - Point.Z) < 0.5;
                    //b_Result &= Math.Abs(point.P - Point.P) < 0.5;
                    //b_Result &= Math.Abs(point.W - Point.W) < 0.5;
                    //b_Result &= Math.Abs(point.R - Point.R) < 0.5;
                    if (b_Result)
                        break;
                    if (stopwatch.ElapsedMilliseconds > 3000)
                        break;
                }
                string[] list_Input = new string[] { Point.X.ToString(), Point.Y.ToString(), Point.Z.ToString(), Point.W.ToString(), Point.P.ToString(), Point.R.ToString(), strMeg };
                if (!b_Result)
                {
                    AlarmMgr.GetIntance().WarnWithDlg((int)SixRobotAlarm.六轴机器人MoveL超时, new string[] { "退出" }, list_Input, "Eston六轴机器人MoveL失败");
                    throw (new Exception("机器人运动失败"));
                }
                return true;
            }
            else
            {
                string[] list_Input = new string[] { Point.X.ToString(), Point.Y.ToString(), Point.Z.ToString(), Point.W.ToString(), Point.P.ToString(), Point.R.ToString(), strMeg };
                AlarmMgr.GetIntance().WarnWithDlg((int)SixRobotAlarm.六轴机器人MoveL超时, new string[] { "退出" }, list_Input, "Eston六轴机器人MoveL失败");
                throw (new Exception("机器人运动失败"));
            }
        }
        //关节运动
        public override bool MoveJog(SixRobotJoint Point, bool bmamual, int Speed = 100, int BlendValue = 500)
        {
            StationCheck(bmamual);
            string Commond = "[MoveToSelectPointb_IFace(1," + "\"" + Point.A1.ToString() + "_" + Point.A2.ToString() + "_" + Point.A3.ToString() + "_" + Point.A4.ToString() + "_" + Point.A5.ToString() + "_" + Point.A6.ToString() + "\"," + "\"" + "0" + "_" + Speed.ToString() + "_" + BlendValue.ToString() + "\"" + "); id = 16]";
            int n_Result = 0;
            string strMeg = "";
            n_Result = Function_SendCmd(Commond, ref strMeg, 60000);
            if (n_Result == 0 && !strMeg.Contains("FAIL"))
            {
                bool b_Result = true;
                Stopwatch stopwatch = new Stopwatch();
                stopwatch.Start();
                while (true)
                {
                    SixRobotJoint point = GetCurrentPos_Joint();
                    b_Result = true;
                    b_Result &= Math.Abs(point.A1 - Point.A1) < 1;
                    b_Result &= Math.Abs(point.A2 - Point.A2) < 1;
                    b_Result &= Math.Abs(point.A3 - Point.A3) < 1;
                    b_Result &= Math.Abs(point.A4 - Point.A4) < 1;
                    b_Result &= Math.Abs(point.A5 - Point.A5) < 1;
                    b_Result &= Math.Abs(point.A6 - Point.A6) < 1;
                    if (b_Result)
                        break;
                    if (stopwatch.ElapsedMilliseconds > 3000)
                        break;
                }
                if (!b_Result)
                {
                    string[] list_Input = new string[] { Point.A1.ToString(), Point.A2.ToString(), Point.A3.ToString(), Point.A4.ToString(), Point.A5.ToString(), Point.A6.ToString(), strMeg };
                    AlarmMgr.GetIntance().WarnWithDlg((int)SixRobotAlarm.六轴机器人MoveJ超时, new string[] { "退出" }, list_Input, "Eston六轴机器人MoveJ失败");
                    throw (new Exception("机器人运动失败"));
                }
                return true;
            }
            else
            {
                string[] list_Input = new string[] { Point.A1.ToString(), Point.A2.ToString(), Point.A3.ToString(), Point.A4.ToString(), Point.A5.ToString(), Point.A6.ToString(), strMeg };
                AlarmMgr.GetIntance().WarnWithDlg((int)SixRobotAlarm.六轴机器人MoveJ超时, new string[] { "退出" }, list_Input, "Eston六轴机器人MoveJ失败");
                throw (new Exception("机器人运动失败"));
            }
        }
        private void StationCheck(bool bmanual)
        {
            if (GlobalVariable.g_StationState == StationState.StationStatePause)
            {
                while (true)
                {
                    Thread.Sleep(10);
                    if (GlobalVariable.g_StationState == StationState.StationStateRun)
                        break;
                    else if (GlobalVariable.g_StationState == StationState.StationStatePause)
                        continue;
                    else
                        throw new Exception("自动运行停止");
                }
            }
            if (GlobalVariable.g_StationState == StationState.StationStateRun && !bmanual)
            {
                return;
            }
            if ((GlobalVariable.g_StationState == StationState.StationStateStop||
                GlobalVariable.g_StationState==StationState.StationStateHoming)&&bmanual)
            {
                return;
            }
            throw new Exception($"当Manual={bmanual}情况下，禁止{GlobalVariable.g_StationState}的运动指令");

        }

        public override int SetServo(bool b_Statue)
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                int nValue = b_Statue ? 1 : 0;
                n_Result = Function_SendCmd($"[SetMotServoStatus_IFace({nValue}); id = 23]", ref strMeg);
                if (n_Result == 0 && strMeg == "[id = 23; Ok]")
                {
                    return 0;
                }
                else
                {
                    return -1;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return -1;

            }
        }
        public override bool ReadServoStatue()
        {
            try
            {
                //int n_Result = 0;
                //string strMeg = "";
                //n_Result = Function_SendCmd($"[GetServoSts_IFace(); id = 24]", ref strMeg);
                //if (n_Result == 0 && strMeg == "[id = 24; Ok; 1]")
                //{
                //    return true;
                //}
                //else
                //{
                //    return false;
                //}
                if(str_ServoInput!="")
                   return IOMgr.GetInstace().ReadIoInBit(str_ServoInput);
                return false;

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return false;

            }

        }
        public override int ResetAlarm()
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"[resetErrorId_IFace(); id = 14]", ref strMeg);
                if (n_Result == 0 && strMeg == "[id = 14; Ok]")
                {
                    return 0;
                }
                else
                {
                    return -1;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return -1;

            }
        }
        public override int SetRunSpeed(int Speed)
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"[setGoableSpeed_IFace({Speed}); id = 11]", ref strMeg);
                if (n_Result == 0 && strMeg == "[id = 11; Ok]")
                {
                    return 0;
                }
                else
                {
                    return -1;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return -1;

            }
        }
        public override int Stop()
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"StopDestPosMotion_IFace(); id = 32", ref strMeg);
                if (n_Result == 0 && strMeg == "[id = 32; Ok]")
                {
                    return 0;
                }
                else
                {
                    return -1;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return -1;

            }

        }

        public override bool IsConnect()
        {
            try
            {
                return b_IsConnect;
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return false;
            }
        }


    }
}